#include "motor_control.h"


#define F 1.8	//2006电机放大系数
#define G 0.3 //宇树放大系数
#define N 20	//鼠标累加放大系数

#define E 0.85//电梯累加系数
#define O 0.02 //图传缩小系数
#define S 0.4 //凑数系数

JointTypedef Test_control;//用于测试的电机结构体
JointTypedef Test_control_2;//用于测试的电机结构体（被控）
float test_Joint;//用于测试的角度
float test_Joint_speed;//用于测试的速度
CascadePID Test_Pid;//测试用双环PID

extern float self_count;

JointTypedef Joint_control[6];//控制电机结构体 调试用
float Joint_Angle[6];


//关节电机的角度
float GM_Joint[2] = {0};
float Last_GM_Joint[2] = {0};
float foc_count[6]; // 电机圈数
float angle_error[6];//电机误差角度



extern const RC_ctrl_t *local_rc_ctrl;

static float Last_out[6] = {0};//低通滤波

float joint1 = 0;//大自转
float joint2 = 0;//上下
float joint3 = 0;//小自转
float joint4 = 0;//一号宇树
float joint5 = 0;//二号宇树
float joint6 = 0;//电梯

float joint1_speed=0.12;
float joint2_speed=0.12;
float joint3_speed=0.12;
float joint4_speed=0.04;
float joint5_speed=0.04;
float joint6_speed=0.17;

extern float add_value_1;
extern float add_value_2;
extern float add_value_3;
extern float add_value_4;
extern float add_value_5;//自定义控制器累加角度值
extern float add_value_6;


float Self_joint1;
float Self_joint2;
float Self_joint3;
float Self_joint4;
float Self_joint5;//自定义角度累加值


//static ChassisTypedef Chassis_control[4];
ChassisTypedef Chassis_control[4];
//int t = 10;
int t = 5;//角度放大系数



int16_t Max_speed=0;//底盘最大速度
key_chassis chassis_move;                                  //底盘键鼠模式速度分解结构体
ramp_function_source_t  chassis_slope[4];				   //斜坡函数
first_order_filter_type_t first_chassis_mouse;             //一阶低通滤波——让键鼠控制下，数据更加平滑
first_order_filter_type_t steer_mouse;             //一阶低通滤波——让键鼠控制下，数据更加平滑
first_order_filter_type_t steer_mouse_1;             //一阶低通滤波——让键鼠控制下，数据更加平滑


float chassis_mouse_filter = 5.5;                  //一阶低通滤波参数——越大越稳，但响应速度降低 //底盘
float steer_mouse_filter = 5.2;                  //一阶低通滤波参数——越大越稳，但响应速度降低 //机械臂
float steer_mouse_filter_1 = 5.2;                  //一阶低通滤波参数——越大越稳，但响应速度降低 //机械臂

int count_mouse = 0; // 记录按键按下次数的计数变量，初始化为0
int count_mouse_1 = 0; // 记录按键按下次数的计数变量，初始化为0//图传
//保存上次的键鼠控制的值
int pump_count = 0;

int last_joint1=0;
int last_joint2=0;
int last_joint3=0;
int last_joint4=0;
int last_joint5=0;
int last_joint6=0;



MOTOR_send cmd1; // joint4电机的发送与接受数据
MOTOR_recv data1;

MOTOR_send cmd2; // joint5电机的发送与接受数据
MOTOR_recv data2;


RGB_Color_TypeDef NOT      = {0,0,0};   //无色
RGB_Color_TypeDef GREEN    = {0,255,0};   //绿色

uint32_t camera_1 = 100;
uint32_t camera_2 = 100 ;
uint32_t last_camera_1;
uint32_t last_camera_2;
float camera_1_speed;
float camera_2_speed;



void GO_M8010_6_Init()
{
    cmd1.id = 1; // 报文数据
    cmd1.mode = 1;
        cmd1.T = 1.2;
		cmd1.W = 0.5;
        cmd1.Pos = 0;
        cmd1.K_P = 4.0;
        cmd1.K_W = 0.06 ;
	
    
        cmd2.id = 0; // 报文数据
        cmd2.mode = 1;
        cmd2.T = 3.2;
		cmd2.W = 0.5;
        cmd2.Pos = 0;
        cmd2.K_P = 2.0;
        cmd2.K_W = 0.06 ;
}
  
void mouse_Init(void)//鼠标值初始化
{
	count_mouse = 0;
	count_mouse_1 = 0;
}

void Joint_Init(void)//init这里有点问题
{
	joint1 = Joint_Angle[0];
//	joint2 = (Joint_Angle[2]  - Joint_Angle[1])/10 ;
//	joint3 = (-Joint_Angle[2]  - Joint_Angle[1])/10;//初始化角度
	
//	 joint4 = cmd1.Pos;
//	 joint5 = cmd2.Pos;

	
	motor_joint_control_1(joint1, joint2, joint3);	
//	motor_joint_control_2(joint4, joint5);
}




/**
 * @param       joint1:five_joint的2006目标角度
 * @param       joint2:four_joint的2006目标角度
 * @param       joint3:three_joint的2006目标角度
 * @param       joint4:宇树电机关节角度
 * @param       joint5:宇树电机关节角度
 * @param       joint6:电梯
 * @retval      集合控制
 **/
void motor_joint_control(float joint1, float joint2, float joint3, float joint4, float joint5, float joint6)
{
    //2006,3508控制
	Joint_control[0].Angle_Pid_set = PidSet(15, 0.08, 0.06, 8000, 4000);  // 角度环参数设置  
	Joint_control[0].Speed_Pid_set = PidSet(34, 0, 0.01, 8000, 4000); // 速度环参数设置	


	Joint_control[1].Angle_Pid_set = PidSet(10, 0.05, 0.05, 8000, 4000);  // 角度环参数设置  
	Joint_control[1].Speed_Pid_set = PidSet(34, 0, 0.01, 8000, 4000); // 速度环参数设置


	Joint_control[2].Angle_Pid_set = PidSet(10, 0.05, 0.05, 8000, 4000);  // 角度环参数设置  
	Joint_control[2].Speed_Pid_set = PidSet(34, 0, 0.01, 8000, 4000); // 速度环参数设置
	

    
	Joint_control[5].Angle_Pid_set = PidSet(63, 0.08, 0.01, 80000, 20000);  // 3508角度环参数设置
	Joint_control[5].Speed_Pid_set = PidSet(26, 0, 0, 80000, 20000); //3508速度环参数设置	
	

	
//	// 角度环PID设置
//	Joint_control[0].Angle_Out = PID_V(&Joint_control[0].Angle_Pid_set, joint1 * t, Joint_Angle[0] * t); // 2006角度环输出     *10是为了增大输出
//	Joint_control[1].Angle_Out = PID_V(&Joint_control[1].Angle_Pid_set, joint2 * t, Joint_Angle[1] * t); 
//	Joint_control[2].Angle_Out = PID_V(&Joint_control[2].Angle_Pid_set, joint3 * t, Joint_Angle[2] * t);
	
	//锥齿
	// 角度环PID设置
	Joint_control[0].Angle_Out = PID_V(&Joint_control[0].Angle_Pid_set, joint1 * t, Joint_Angle[0] * t); // 2006角度环输出     *10是为了增大输出
	Joint_control[1].Angle_Out = PID_V(&Joint_control[1].Angle_Pid_set, joint3 * t + joint2 * t, Joint_Angle[1] * t); 
	Joint_control[2].Angle_Out = PID_V(&Joint_control[2].Angle_Pid_set, joint3 * t - joint2 * t, Joint_Angle[2] * t);		
	
	// 速度环PID设置
	Joint_control[0].Speed_Out = PID_V(&Joint_control[0].Speed_Pid_set, Joint_control[0].Angle_Out, Joint_control[0].Now_Motor_Data->speed_rpm); // 2006速度环输出
	Joint_control[1].Speed_Out = PID_V(&Joint_control[1].Speed_Pid_set, Joint_control[1].Angle_Out, Joint_control[1].Now_Motor_Data->speed_rpm);
	Joint_control[2].Speed_Out = PID_V(&Joint_control[2].Speed_Pid_set, Joint_control[2].Angle_Out, Joint_control[2].Now_Motor_Data->speed_rpm);	
	
	
	Joint_control[5].Angle_Out = PID_V(&Joint_control[5].Angle_Pid_set, joint6 * t, Joint_Angle[5] * t); // 3508角度环输出  ID:6	
	Joint_control[5].Speed_Out = PID_V(&Joint_control[5].Speed_Pid_set, Joint_control[5].Angle_Out, Joint_control[5].Now_Motor_Data->speed_rpm); // 3508速度环输出	

	CAN_cmd_arm2(-Joint_control[5].Speed_Out, Joint_control[5].Speed_Out);    //3508电机输出
	CAN_cmd_arm1(Joint_control[0].Speed_Out, Joint_control[1].Speed_Out, Joint_control[2].Speed_Out, 0); // 2006电机输出
    cmd1.Pos = joint4/360*6.28*6.33;
    cmd2.Pos = joint5/360*6.28*6.33;


	
    SERVO_Send_recv(&cmd1, &data1); // 发送数据与接收数据
    SERVO_Send_recv(&cmd2, &data2); // 发送数据与接收数据		
}

/**
 * @param       joint1:1号的2006目标角度
 * @param       joint2:2号的2006目标角度度
 * @param       joint3:3号的2006目标角度
 * @param       joint4:电梯控制
 * @retval      1号分控制
 **/
void motor_joint_control_1(float joint1, float joint2, float joint3)
{
	
    //2006,3508控制
	Joint_control[0].Angle_Pid_set = PidSet(19.2, 0.04, 0.51, 8000, 4000);  // 角度环参数设置  
	Joint_control[0].Speed_Pid_set = PidSet(26.7, 0.05, 0.15, 8000, 4000); // 速度环参数设置	



	Joint_control[1].Angle_Pid_set = PidSet(35.5, 0.08, 0.18, 8000, 4000);  // 角度环参数设置  
	Joint_control[1].Speed_Pid_set = PidSet(32, 0.05, 0.01, 8000, 4000); // 速度环参数设置


	Joint_control[2].Angle_Pid_set = PidSet(35.5, 0.08, 0.18, 8000, 4000);  // 角度环参数设置  
	Joint_control[2].Speed_Pid_set = PidSet(32, 0.05, 0.01, 8000, 4000); // 速度环参数设置




	
//	// 角度环PID设置
//	Joint_control[0].Angle_Out = PID_V(&Joint_control[0].Angle_Pid_set, joint1 * t, Joint_Angle[0] * t); // 2006角度环输出     *10是为了增大输出
//	Joint_control[1].Angle_Out = PID_V(&Joint_control[1].Angle_Pid_set, joint2 * t, Joint_Angle[1] * t); 
//	Joint_control[2].Angle_Out = PID_V(&Joint_control[2].Angle_Pid_set, joint3 * t, Joint_Angle[2] * t);
	
	//锥齿
	// 角度环PID设置
	Joint_control[0].Angle_Out = PID_V(&Joint_control[0].Angle_Pid_set, joint1 * t, Joint_Angle[0] * t); // 2006角度环输出     *10是为了增大输出
	Joint_control[1].Angle_Out = PID_V(&Joint_control[1].Angle_Pid_set, -joint3 * t - joint2 * t, Joint_Angle[1] * t); 
	Joint_control[2].Angle_Out = PID_V(&Joint_control[2].Angle_Pid_set, -joint3 * t + joint2 * t, Joint_Angle[2] * t);	
	
	// 速度环PID设置
	Joint_control[0].Speed_Out = PID_V(&Joint_control[0].Speed_Pid_set, Joint_control[0].Angle_Out, Joint_control[0].Now_Motor_Data->speed_rpm); // 2006速度环输出
	Joint_control[1].Speed_Out = PID_V(&Joint_control[1].Speed_Pid_set, Joint_control[1].Angle_Out, Joint_control[1].Now_Motor_Data->speed_rpm);
	Joint_control[2].Speed_Out = PID_V(&Joint_control[2].Speed_Pid_set, Joint_control[2].Angle_Out, Joint_control[2].Now_Motor_Data->speed_rpm);	
	
	

	CAN_cmd_arm1(Joint_control[0].Speed_Out, Joint_control[1].Speed_Out, Joint_control[2].Speed_Out, 0); // 2006电机输出

}

/**
 * @param       joint1:1号宇树电机目标角度
 * @param       joint2:2号宇树电机目标角度
 * @param       joint3:电梯的升降
 * @retval      2号分控制
 **/
void motor_joint_control_2(float joint1, float joint2)
{


	

    cmd1.Pos = joint4/360*6.28*6.33;
    cmd2.Pos = -joint5/360*6.28*6.33;


	
    SERVO_Send_recv(&cmd1, &data1); // 发送数据与接收数据
    SERVO_Send_recv(&cmd2, &data2); // 发送数据与接收数据	
}




/**
 * @brief        返回六轴实时角度
 * @param        none
 * @retval       六轴实时角度
 **/
float *get_joint_current_angle(void)
{
	Joint_control[0].Now_Motor_Data = get_arm_motor_measure_point(0); // 读取Joint1的电机数据(2006)
	Joint_control[1].Now_Motor_Data = get_arm_motor_measure_point(1); // 读取Joint2的电机数据(2006)
	Joint_control[2].Now_Motor_Data = get_arm_motor_measure_point(2); // 读取Joint3的电机数据(2006)
	Joint_control[3].Now_Motor_Data = get_arm_motor_measure_point(3); // 读取Joint4的电机数据(2006)
	Joint_control[4].Now_Motor_Data = get_arm_motor_measure_point(4); // 读取Joint5的电机数据(3508)
	Joint_control[5].Now_Motor_Data = get_arm_motor_measure_point(5); // 读取Joint6的电机数据(3508)




	Joint_Angle[1] = Joint_control[1].Now_Motor_Data->ecd + foc_count[1] * 8192;	
	if(Joint_Angle[1] >=0)                                                            //如果这个条件使用，在elsed中处理一下控制角度的周期
			{
				Joint_Angle[1] = Joint_Angle[1]/(8192*18.00f)*360;       //输出轴的机械刻度换算成角度 18为机械刻度
			}
			else
			{
				Joint_Angle[1] = Joint_Angle[1]/(8192*18.00f)*360;        //输出轴的机械刻度换算成角度 18为机械刻度
			}	
	
			
	Joint_Angle[0] = Joint_control[0].Now_Motor_Data->ecd + foc_count[0] * 8192;	
	if(Joint_Angle[0] >=0)                                                            //如果这个条件使用，在elsed中处理一下控制角度的周期
			{
				Joint_Angle[0] = Joint_Angle[0]/(8192*18.00f)*360;       //输出轴的机械刻度换算成角度 18为机械刻度
			}
			else
			{
				Joint_Angle[0] = Joint_Angle[0]/(8192*18.00f)*360;        //输出轴的机械刻度换算成角度 18为机械刻度
			}	
	
	
	Joint_Angle[2] = Joint_control[2].Now_Motor_Data->ecd + foc_count[2] * 8192;	
	if(Joint_Angle[2] >=0)                                                            //如果这个条件使用，在elsed中处理一下控制角度的周期
			{
				Joint_Angle[2] = Joint_Angle[2]/(8192*18.00f)*360;       //输出轴的机械刻度换算成角度 18为机械刻度
			}
			else
			{
				Joint_Angle[2] = Joint_Angle[2]/(8192*18.00f)*360;        //输出轴的机械刻度换算成角度 18为机械刻度
			}
	
			
	Joint_Angle[3] = (Joint_control[3].Now_Motor_Data->ecd/8192.0) + foc_count[3] / 8192;			
	Joint_Angle[4] = (Joint_control[5].Now_Motor_Data->ecd/8191.0)*18.94/6.00;
//	Joint_Angle[5] = (Joint_control[5].Now_Motor_Data->ecd/8191.0)*18.94/6.00;
			
			
	Joint_Angle[5] = Joint_control[5].Now_Motor_Data->ecd + foc_count[5] * 8192;	
	if(Joint_Angle[5] >=0)                                                            //如果这个条件使用，在elsed中处理一下控制角度的周期
			{
				Joint_Angle[5] = Joint_Angle[5]/(8192*18.00f)*360;       //输出轴的机械刻度换算成角度 18为机械刻度
			}
			else
			{
				Joint_Angle[5] = Joint_Angle[5]/(8192*18.00f)*360;        //输出轴的机械刻度换算成角度 18为机械刻度
			}
			
    
    GM_Joint[0] = (data1.motor_recv_data.fbk.pos/6.33/32768)*6.28;
    GM_Joint[1] = (data2.motor_recv_data.fbk.pos/6.33/32768)*6.28;
    
    if(GM_Joint[0] - Last_GM_Joint[0] > 6.28 || GM_Joint[0] - Last_GM_Joint[0] < -6.28) GM_Joint[0] = Last_GM_Joint[0];
    if(GM_Joint[1] - Last_GM_Joint[1] > 6.28 || GM_Joint[1] - Last_GM_Joint[1] < -6.28) GM_Joint[0] = Last_GM_Joint[1];
    
    Last_GM_Joint[0] = GM_Joint[0];
    Last_GM_Joint[1] = GM_Joint[1];
	//保存上次的键鼠控制的值
int last_joint1=Joint_Angle[0];
int last_joint2=0;
int last_joint3=0;
int last_joint4=0;
int last_joint5=0;
int last_joint6=0;

	return Joint_Angle;
}
/**
 * @brief   读取底盘电机数据
 * @param   none
 * @retval  电机各项数据
 **/
void get_chassis_data(void)
{
	Chassis_control[0].Now_Motor_Data = get_chassis_motor_measure_point(0);
	Chassis_control[1].Now_Motor_Data = get_chassis_motor_measure_point(1);
	Chassis_control[2].Now_Motor_Data = get_chassis_motor_measure_point(2);
	Chassis_control[3].Now_Motor_Data = get_chassis_motor_measure_point(3);
}




void max_speed()			//鼠标控制底盘加减速
{
	
				if((local_rc_ctrl->key.v&KEY_PRESSED_OFFSET_SHIFT))
				{
					 Max_speed = 11000;
					 joint1_speed=0.17 * F;
					 joint2_speed=0.17 * F;
					 joint3_speed=0.17 * F;
					 joint4_speed=0.02 * F;
					 joint5_speed=0.04 * F;
					 joint6_speed=0.17 * F;
					
				}
				else if(local_rc_ctrl->key.v&KEY_PRESSED_OFFSET_CTRL)
				{
					Max_speed = 2500;
				}
				else 
				{
					Max_speed = 5500;
					 joint1_speed=0.17;
					 joint2_speed=0.17;
					 joint3_speed=0.17;
					 joint4_speed=0.02;
					 joint5_speed=0.04;
					 joint6_speed=0.17;
					 camera_1_speed = 0.05;
					 camera_2_speed = 0.05;
				}
							  

		

}

void max_speed_1()	//鼠标控制底盘加减速
{	
				if((judge_sensor_info.RemoteControlCommand.keyboard_value&KEY_PRESSED_OFFSET_SHIFT))
				{
					 Max_speed = 11000;
					 joint1_speed=0.17 * F;
					 joint2_speed=0.17 * F;
					 joint3_speed=0.17 * F;
					 joint4_speed=0.02 * F;
					 joint5_speed=0.04 * F;
					 joint6_speed=0.17 * F;
					
				}
				else if(judge_sensor_info.RemoteControlCommand.keyboard_value&KEY_PRESSED_OFFSET_CTRL)
				{
					Max_speed = 2500;
				}
				else
				{
					Max_speed = 5500;
					 joint1_speed=0.17;
					 joint2_speed=0.17;
					 joint3_speed=0.17;
					 joint4_speed=0.02;
					 joint5_speed=0.04;
					 joint6_speed=0.17;
					 camera_1_speed = 0.07;
					 camera_2_speed = 0.07;
				}
			}



/**
 * @brief          麦轮底盘解算
 * @param[in]      SPEED_X: X速度
 * @param[in]      SPEED_Y: Y速度
 * @param[in]      SPEED_W_Z: 旋转速度
 * @author     		Chen_son
 */
void ChassisMove(int SPEED_X, int SPEED_Y, int SPEED_W_Z)
{
		max_speed();
	Chassis_control[0].WheelSpeed = Max_speed/660* (SPEED_X - SPEED_Y + SPEED_W_Z);
	Chassis_control[1].WheelSpeed = Max_speed/660 * (SPEED_X + SPEED_Y + SPEED_W_Z);
	Chassis_control[2].WheelSpeed = Max_speed/660 * (-SPEED_X + SPEED_Y + SPEED_W_Z);
	Chassis_control[3].WheelSpeed = Max_speed/660* (-SPEED_X - SPEED_Y + SPEED_W_Z);
}


void ChassisMove_1(int SPEED_X, int SPEED_Y, int SPEED_W_Z)
{
		max_speed_1();
	Chassis_control[0].WheelSpeed = Max_speed/660* (SPEED_X - SPEED_Y + SPEED_W_Z);
	Chassis_control[1].WheelSpeed = Max_speed/660 * (SPEED_X + SPEED_Y + SPEED_W_Z);
	Chassis_control[2].WheelSpeed = Max_speed/660 * (-SPEED_X + SPEED_Y + SPEED_W_Z);
	Chassis_control[3].WheelSpeed = Max_speed/660* (-SPEED_X - SPEED_Y + SPEED_W_Z);
}

void chassis_key()                          //底盘键鼠平移控制
{
	if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_W)
	{
//	chassis_move.Vw = 660;
	chassis_move.Vw = 500;
	}
	else
	{
		chassis_move.Vw = 0;
	}
	if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_S)
	{
//		chassis_move.Vs = -660;
		chassis_move.Vs = -500;
	}
	else
	{
		chassis_move.Vs = 0; 
	}
	if(local_rc_ctrl->key.v& KEY_PRESSED_OFFSET_D)
	{
//		chassis_move.Vd = 880;
		chassis_move.Vd = 600;
	}
	else
	{
		chassis_move.Vd = 0; 
	}
	if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_A)
	{
//		chassis_move.Va = -880;
		chassis_move.Va = -600;		
	}
	else
	{
		chassis_move.Va = 0;
	}

	
	
	
	
	chassis_move.set_x = chassis_move.Va + chassis_move.Vd;
	chassis_move.set_y = chassis_move.Vw + chassis_move.Vs;
	
	if((local_rc_ctrl->key.v& KEY_PRESSED_OFFSET_W) == 0 &&(local_rc_ctrl->key.v& KEY_PRESSED_OFFSET_S) == 0)   //没有按下时归零
	{
		chassis_move.set_y = 0;
	}
	if((local_rc_ctrl->key.v& KEY_PRESSED_OFFSET_A) == 0&&(local_rc_ctrl->key.v& KEY_PRESSED_OFFSET_D) == 0)
	{
		chassis_move.set_x = 0;
	}
		if((local_rc_ctrl->key.v& KEY_PRESSED_OFFSET_Q) == 0&&(local_rc_ctrl->key.v& KEY_PRESSED_OFFSET_E) == 0)
	{
		chassis_move.speed_w_z = 0;
	}
	
	
	ramp_calc(&chassis_slope[0], chassis_move.set_x);      //斜坡数据处理，让底盘控制数据更加平滑
	ramp_calc(&chassis_slope[1], chassis_move.set_y);
	ramp_calc(&chassis_slope[2], chassis_move.speed_w_z);
	first_order_filter_cali(&first_chassis_mouse,local_rc_ctrl->mouse.x);      //一阶低通滤波处理鼠标反馈值

}


void chassis_key_1()                          //底盘键鼠平移控制//图传
{
	if(judge_sensor_info.RemoteControlCommand.keyboard_value & KEY_PRESSED_OFFSET_W)
	{
//	chassis_move.Vw = 660;
	chassis_move.Vw = 500;
	}
	else
	{
		chassis_move.Vw = 0;
	}
	if(judge_sensor_info.RemoteControlCommand.keyboard_value & KEY_PRESSED_OFFSET_S)
	{
//		chassis_move.Vs = -660;
		chassis_move.Vs = -500;
	}
	else
	{
		chassis_move.Vs = 0; 
	}
	if(judge_sensor_info.RemoteControlCommand.keyboard_value& KEY_PRESSED_OFFSET_D)
	{
//		chassis_move.Vd = 880;
		chassis_move.Vd = 600;
	}
	else
	{
		chassis_move.Vd = 0; 
	}
	if(judge_sensor_info.RemoteControlCommand.keyboard_value& KEY_PRESSED_OFFSET_A)
	{
//		chassis_move.Va = -880;
		chassis_move.Va = -600;		
	}
	else
	{
		chassis_move.Va = 0;
	}

	
	
	
	
	chassis_move.set_x = chassis_move.Va + chassis_move.Vd;
	chassis_move.set_y = chassis_move.Vw + chassis_move.Vs;
	
	if((judge_sensor_info.RemoteControlCommand.keyboard_value & KEY_PRESSED_OFFSET_W) == 0 &&(judge_sensor_info.RemoteControlCommand.keyboard_value & KEY_PRESSED_OFFSET_S) == 0)   //没有按下时归零
	{
		chassis_move.set_y = 0;
	}
	if((judge_sensor_info.RemoteControlCommand.keyboard_value& KEY_PRESSED_OFFSET_A) == 0&&(judge_sensor_info.RemoteControlCommand.keyboard_value& KEY_PRESSED_OFFSET_D) == 0)
	{
		chassis_move.set_x = 0;
	}
		if((judge_sensor_info.RemoteControlCommand.keyboard_value& KEY_PRESSED_OFFSET_Q) == 0&&(judge_sensor_info.RemoteControlCommand.keyboard_value& KEY_PRESSED_OFFSET_E) == 0)
	{
		chassis_move.speed_w_z = 0;
	}
	
	
	ramp_calc(&chassis_slope[0], chassis_move.set_x);      //斜坡数据处理，让底盘控制数据更加平滑
	ramp_calc(&chassis_slope[1], chassis_move.set_y);
	ramp_calc(&chassis_slope[2], chassis_move.speed_w_z);
	first_order_filter_cali(&first_chassis_mouse,judge_sensor_info.RemoteControlCommand.mouse_x);      //一阶低通滤波处理鼠标反馈值

}

/**
 * @brief          单电机速度超阈值处理函数
 * @param[in]      Speed_3508:   3508解算后的目标速度
 * @param[out]     返回值代表是否超过3508阈值
 * @author     		Chen
 */
int LIMIT_3508(int Speed_3508, int Speed_MAX)
{
	if (Speed_3508 > Speed_MAX)
	{
		Speed_3508 = Speed_MAX;
		return 1;
	}
	else if (Speed_3508 < -Speed_MAX)
	{
		Speed_3508 = -Speed_MAX;
		return 1;
	}
	else
	{
		return 0;
	}
}

/*鼠标控制累加程序*/
void mouse ()
{

	first_order_filter_init(&steer_mouse, 6, steer_mouse_filter);       //一阶低通滤波初始化
	first_order_filter_cali(&steer_mouse,count_mouse); 
	
	if (local_rc_ctrl->mouse.press_l==1)
		{
    count_mouse+=1; // 自加1
    }
	else if (local_rc_ctrl->mouse.press_r==1)
			{
    count_mouse-=1; // 自减1
    }
			
	
	
}
					 
/*鼠标控制累加程序*/
void mouse_1 ()
{

	first_order_filter_init(&steer_mouse_1, 6, steer_mouse_filter_1);       //一阶低通滤波初始化
	first_order_filter_cali(&steer_mouse_1,count_mouse_1); 
	
	if (judge_sensor_info.RemoteControlCommand.left_button_down==1)
		{
    count_mouse_1+=1; // 自加1
    }
	else if (judge_sensor_info.RemoteControlCommand.right_button_down==1)
			{
    count_mouse_1-=1; // 自减1
    }
			
	
	
}



/**/

void  Read_joint_control(int i)
{
			mouse();
    		
			if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_Z)
			  {
           joint1 = count_mouse*joint1_speed+last_joint1;  


				}

			
/*************************************************************/
			else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_X)
			  {
           joint2 = -count_mouse*joint2_speed+last_joint2;  

				}

/*************************************************************/
		else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_C)
			  {
           joint3 = -count_mouse*joint3_speed+last_joint3;  

				}

/*************************************************************/
		else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_V)
			  {
           joint4 = count_mouse*joint4_speed+last_joint4;  

				}

/*************************************************************/
			else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_B)
			  {
           joint5 = count_mouse*joint5_speed+last_joint5;  

				}
			  
		else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_G)
			  {
           joint6 = count_mouse*joint6_speed+last_joint6;  

				}
//		else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_F)
//			  {
//           joint6 = last_joint6 - 0.1 * E;  

//				}
	   else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_Q)
			  {
           camera_1 = count_mouse*camera_1_speed+last_camera_1;  


				}
	else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_E)
			  {
           camera_2 = count_mouse*camera_2_speed+last_camera_2;  
				}
		else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_R)
			  {
           mouse_Init();
		   Joint_Init();

				}

				
				
			else{
				count_mouse=0;
                last_joint1 = joint1 ;
                last_joint2 = joint2 ;
                last_joint3 = joint3 ;			
                last_joint4 = joint4 ;
                last_joint5 = joint5 ;
//                joint6 = 0;
			   last_joint6 = joint6;
			   last_camera_1 = camera_1;	
			   last_camera_2 = camera_2;
				
			   }
				
//          if(i == 0) motor_joint_control( Joint_Angle[0]+joint1, Joint_Angle[1]+joint2, Joint_Angle[3]+joint3, joint4, joint5, Joint_Angle[5]+joint6);    
//          else if(i == 1) motor_joint_control( Joint_Angle[0]+joint1, Joint_Angle[1]+joint2, Joint_Angle[3]+joint3, 1.82+joint4, 1.67+joint5, Joint_Angle[5]+joint6);

		




}

/**/

void  Read_joint_control_1(int i)//图传
{
			mouse_1 ();
    		
			if(judge_sensor_info.RemoteControlCommand.keyboard_value & KEY_PRESSED_OFFSET_Z)
			  {
           joint1 = count_mouse_1*joint1_speed*O+last_joint1;  


				}

			
/*************************************************************/
			else if(judge_sensor_info.RemoteControlCommand.keyboard_value  & KEY_PRESSED_OFFSET_X)
			  {
           joint2 = -count_mouse_1*joint2_speed*O+last_joint2;  

				}

/*************************************************************/
		   else if(judge_sensor_info.RemoteControlCommand.keyboard_value  & KEY_PRESSED_OFFSET_C)
			  {
           joint3 = -count_mouse_1*joint3_speed*O+last_joint3;  

				}

/*************************************************************/
		   else if(judge_sensor_info.RemoteControlCommand.keyboard_value  & KEY_PRESSED_OFFSET_V)
			  {
           joint4 = count_mouse_1*joint4_speed*O+last_joint4;  

				}

/*************************************************************/
		   else if(judge_sensor_info.RemoteControlCommand.keyboard_value  & KEY_PRESSED_OFFSET_B)
			  {
           joint5 = count_mouse_1*joint5_speed*O+last_joint5;  

				}
			  
		   else if(judge_sensor_info.RemoteControlCommand.keyboard_value  & KEY_PRESSED_OFFSET_G)
			  {
           joint6 = count_mouse_1*joint6_speed*O+last_joint6;  

				}
//		else if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_F)
//			  {
//           joint6 = last_joint6 - 0.1 * E;  

//				}
	       else if(judge_sensor_info.RemoteControlCommand.keyboard_value  & KEY_PRESSED_OFFSET_Q)
			  {
           camera_1 = count_mouse_1*camera_1_speed*O+last_camera_1;  


				}
		   else if(judge_sensor_info.RemoteControlCommand.keyboard_value & KEY_PRESSED_OFFSET_E)
			  {
           camera_2 = count_mouse_1*camera_2_speed*O+last_camera_2;  
				}
		    else if(judge_sensor_info.RemoteControlCommand.keyboard_value & KEY_PRESSED_OFFSET_R)
			  {
           mouse_Init();
		   Joint_Init();

				}

				
				
			else{
				count_mouse_1=0;
                last_joint1 = joint1 ;
                last_joint2 = joint2 ;
                last_joint3 = joint3 ;			
                last_joint4 = joint4 ;
                last_joint5 = joint5 ;
//                joint6 = 0;
			   last_joint6 = joint6;
			   last_camera_1 = camera_1;	
			   last_camera_2 = camera_2;
				
			   }
}

void read_unitee(void)//读取宇树电机数据
{
	extract_data(&data1);
	extract_data(&data2);	
}



void Chassis_Control_Task()
{

	first_order_filter_init(&first_chassis_mouse, 8, chassis_mouse_filter);       //一阶低通滤波初始化
	ramp_init(&chassis_slope[0], 0.05, 660, -660);
	ramp_init(&chassis_slope[1], 0.05, 660, -660);
	ramp_init(&chassis_slope[2], 0.1, 660, -660);


  	
	// PidSet(float Kp,float Ki,float Kd,float max_out,float max_iout);
	Chassis_control[0].Speed_Pid_set = PidSet(4, 0.3, 0.6, 16000, 8000); // 初始化底盘pid参数
	Chassis_control[1].Speed_Pid_set = PidSet(4, 0.3, 0.6, 16000, 8000);
	Chassis_control[2].Speed_Pid_set = PidSet(4, 0.3, 0.6, 16000, 8000);
	Chassis_control[3].Speed_Pid_set = PidSet(4, 0.3, 0.6, 16000, 8000);
	chassis_key();//键盘值解算
	// 获得电机数据
	get_chassis_data();
	ChassisMove(local_rc_ctrl->rc.ch[2] + chassis_slope[0].out*10,
	local_rc_ctrl->rc.ch[3]+ chassis_slope[1].out*10,
	local_rc_ctrl->rc.ch[0] +chassis_slope[2].out*5+ first_chassis_mouse.out * 50);
	/*麦轮解算后pid逼近*/
	Chassis_control[0].Chassis_3508_Current = (int16_t)PID_V(&Chassis_control[0].Speed_Pid_set, Chassis_control[0].WheelSpeed, Chassis_control[0].Now_Motor_Data->speed_rpm); // lf
	Chassis_control[1].Chassis_3508_Current = (int16_t)PID_V(&Chassis_control[1].Speed_Pid_set, Chassis_control[1].WheelSpeed, Chassis_control[1].Now_Motor_Data->speed_rpm); // lb
	Chassis_control[2].Chassis_3508_Current = (int16_t)PID_V(&Chassis_control[2].Speed_Pid_set, Chassis_control[2].WheelSpeed, Chassis_control[2].Now_Motor_Data->speed_rpm); // rf
	Chassis_control[3].Chassis_3508_Current = (int16_t)PID_V(&Chassis_control[3].Speed_Pid_set, Chassis_control[3].WheelSpeed, Chassis_control[3].Now_Motor_Data->speed_rpm); // rb


	canTX_chassis(Chassis_control[0].Chassis_3508_Current, Chassis_control[1].Chassis_3508_Current, Chassis_control[2].Chassis_3508_Current, Chassis_control[3].Chassis_3508_Current);


}


void Chassis_Control_Task_1()
{

	first_order_filter_init(&first_chassis_mouse, 8, chassis_mouse_filter);       //一阶低通滤波初始化
	ramp_init(&chassis_slope[0], 0.05, 660, -660);
	ramp_init(&chassis_slope[1], 0.05, 660, -660);
	ramp_init(&chassis_slope[2], 0.1, 660, -660);


  	
	// PidSet(float Kp,float Ki,float Kd,float max_out,float max_iout);
	Chassis_control[0].Speed_Pid_set = PidSet(4, 0.3, 0.6, 16000, 8000); // 初始化底盘pid参数
	Chassis_control[1].Speed_Pid_set = PidSet(4, 0.3, 0.6, 16000, 8000);
	Chassis_control[2].Speed_Pid_set = PidSet(4, 0.3, 0.6, 16000, 8000);
	Chassis_control[3].Speed_Pid_set = PidSet(4, 0.3, 0.6, 16000, 8000);
	chassis_key_1();
	// 获得电机数据
	get_chassis_data();
	ChassisMove_1(local_rc_ctrl->rc.ch[2] + chassis_slope[0].out*10,
	local_rc_ctrl->rc.ch[3]+ chassis_slope[1].out*10,
	local_rc_ctrl->rc.ch[0] +chassis_slope[2].out*5+ first_chassis_mouse.out * 50);
	/*麦轮解算后pid逼近*/
	Chassis_control[0].Chassis_3508_Current = (int16_t)PID_V(&Chassis_control[0].Speed_Pid_set, Chassis_control[0].WheelSpeed, Chassis_control[0].Now_Motor_Data->speed_rpm); // lf
	Chassis_control[1].Chassis_3508_Current = (int16_t)PID_V(&Chassis_control[1].Speed_Pid_set, Chassis_control[1].WheelSpeed, Chassis_control[1].Now_Motor_Data->speed_rpm); // lb
	Chassis_control[2].Chassis_3508_Current = (int16_t)PID_V(&Chassis_control[2].Speed_Pid_set, Chassis_control[2].WheelSpeed, Chassis_control[2].Now_Motor_Data->speed_rpm); // rf
	Chassis_control[3].Chassis_3508_Current = (int16_t)PID_V(&Chassis_control[3].Speed_Pid_set, Chassis_control[3].WheelSpeed, Chassis_control[3].Now_Motor_Data->speed_rpm); // rb


	canTX_chassis(Chassis_control[0].Chassis_3508_Current, Chassis_control[1].Chassis_3508_Current, Chassis_control[2].Chassis_3508_Current, Chassis_control[3].Chassis_3508_Current);


}


void Chassis_Stop(void)
{
	canTX_chassis(0, 0, 0, 0);
}

/**
 * @brief 无刷电机运行圈数判断
 * @param none
 * @retval none
 */
void foc_count_judge()
{
	angle_error [0] = (Joint_control[0].Now_Motor_Data->ecd - Joint_control[0].Now_Motor_Data->last_ecd) ;
	angle_error [1] = (Joint_control[1].Now_Motor_Data->ecd - Joint_control[1].Now_Motor_Data->last_ecd) ;
	angle_error [2] = (Joint_control[2].Now_Motor_Data->ecd - Joint_control[2].Now_Motor_Data->last_ecd) ;	
	angle_error [5] = (Joint_control[5].Now_Motor_Data->ecd - Joint_control[5].Now_Motor_Data->last_ecd) ;	
	
	
	if (angle_error [0] > 6000)
	{
		foc_count[0]--;
	}
	else if (angle_error [0] < -6000)
	{
		foc_count[0]++;
	}

	if (angle_error [1] > 6000)
	{
		foc_count[1]--;
	}
	else if (angle_error [1] < -6000)
	{
		foc_count[1]++;
	}
		
	if (angle_error [2] > 6000)
	{
		foc_count[2]--;
	}
	else if (angle_error [2] < -6000)
	{
		foc_count[2]++;
	}

	if (Joint_control[3].Now_Motor_Data->ecd - Joint_control[3].Now_Motor_Data->last_ecd > 6000)
	{
		foc_count[3]--;
	}
	else if (Joint_control[3].Now_Motor_Data->ecd - Joint_control[3].Now_Motor_Data->last_ecd < -6000)
	{
		foc_count[3]++;
	}
	if (Joint_control[4].Now_Motor_Data->ecd - Joint_control[4].Now_Motor_Data->last_ecd > 6000)
	{
		foc_count[4]--;
	}
	else if (Joint_control[4].Now_Motor_Data->ecd - Joint_control[4].Now_Motor_Data->last_ecd < -6000)
	{
		foc_count[4]++;
	}
	
	if (angle_error [5] > 6000)
	{
		foc_count[5]--;
	}
	else if (angle_error [5] < -6000)
	{
		foc_count[5]++;
	}

}


/**
 * @brief 低通滤波（缓启缓停）
 * @param[in]  PID输出的电流值,i为第i个joint
 * @param[out] 滤波后的值
 */
float pid_out_flr(float in,int i)
{
	float out = 0;
	out = 0.001 * in + 0.999 * Last_out[i];
	Last_out[i] = out;
	return out;
}


/**
* @param       joint1:电梯升降控制参数
 * @retval      3号分控制
 **/
void elevator(float joint1)
{	
	Joint_control[5].Angle_Pid_set = PidSet(68, 0.08, 0.1, 80000, 20000);  // 3508角度环参数设置
	Joint_control[5].Speed_Pid_set = PidSet(23.2, 0.01, 0.11, 80000, 20000); //3508速度环参数设置	
	
	Joint_control[5].Angle_Out = PID_V(&Joint_control[5].Angle_Pid_set, joint1 * t, Joint_Angle[5] * t); // 3508角度环输出  ID:6	
	Joint_control[5].Speed_Out = PID_V(&Joint_control[5].Speed_Pid_set, Joint_control[5].Angle_Out, Joint_control[5].Now_Motor_Data->speed_rpm); // 3508速度环输出	
	
	CAN_cmd_arm2(-Joint_control[5].Speed_Out, Joint_control[5].Speed_Out);    //3508电机输出	
}

void wrist_control(void)//控制机械臂2006电机
{

	
	if(50 < local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] <= 230)
	{
		joint1 += 0.05 * F;
	}
	if(231 <= local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] <= 422)
	{
		joint1 += 0.1 * F;
	}	
	if(423 <= local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] <= 660)
	{
		joint1 += 0.15 * F;
	}
	if(-50 > local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] >= -230)
	{
		joint1 -= 0.05 * F;
	}
	if(-231 >= local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] >= -422)
	{
		joint1 -= 0.1 * F;
	}	
	if(-423 >= local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] >= -660)
	{
		joint1 -= 0.15 * F;
	}	

	
	if(50 < local_rc_ctrl->rc.ch[3] &&local_rc_ctrl->rc.ch[3] <= 230)
	{
		joint2 += 0.05 * F;
	}
	if(231 <= local_rc_ctrl->rc.ch[3] &&local_rc_ctrl->rc.ch[3] <= 422)
	{
		joint2 += 0.1 * F;
	}	
	if(423 <= local_rc_ctrl->rc.ch[3] &&local_rc_ctrl->rc.ch[3] <= 660)
	{
		joint2 += 0.15 * F;
	}
	if(-50 > local_rc_ctrl->rc.ch[3] &&local_rc_ctrl->rc.ch[3] >= -230)
	{
		joint2 -= 0.05 * F;
	}
	if(-231 >= local_rc_ctrl->rc.ch[3] &&local_rc_ctrl->rc.ch[3] >= -422)
	{
		joint2 -= 0.1 * F;
	}	
	if(-423 >= local_rc_ctrl->rc.ch[3] &&local_rc_ctrl->rc.ch[3] >= -660)
	{
		joint2 -= 0.15 * F;
	}		
	
	
	if(50 < local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] <= 230)
	{
		joint3 += 0.1 * F;
	}
	if(231 <= local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] <= 422)
	{
		joint3 += 0.15 * F;
	}	
	if(423 <= local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] <= 660)
	{
		joint3 += 0.2 * F;
	}
	if(-50 > local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] >= -230)
	{
		joint3 -= 0.1 * F;
	}
	if(-231 >= local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] >= -422)
	{
		joint3 -= 0.15 * F;
	}	
	if(-423 >= local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] >= -660)
	{
		joint3 -= 0.2 * F;
	}	




	
	motor_joint_control_1(joint1, joint2, joint3);

}


void elevator_control_ADD(void)//控制机械臂升降,左右摇摆，宇树,累加值控制
{
	

//	Limit_float(&joint4, 100, -140);
//	Limit_float(&joint5, 100, -130);	
	

	if(50 < local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] <= 230)
	{
		joint5 += 0.05 * G;
	}
	if(231 <= local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] <= 422)
	{
		joint5 += 0.1 * G;
	}	
	if(423 <= local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] <= 660)
	{
		joint5 += 0.15 * G;
	}
	if(-50 > local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] >= -230)
	{
		joint5 -= 0.05 * G;
	}
	if(-231 >= local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] >= -422)
	{
		joint5 -= 0.1 * G;
	}	
	if(-423 >= local_rc_ctrl->rc.ch[2] &&local_rc_ctrl->rc.ch[2] >= -660)
	{
		joint5 -= 0.15 * G;
	}
	//为避免摆动幅度过大 采取分段算法 后续可继续优化分类
	if(50 < local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] <= 230)
	{
		joint4 += 0.05 * G;
	}
	if(231 <= local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] <= 422)
	{
		joint4 += 0.1 * G;
	}	
	if(423 <= local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] <= 660)
	{
		joint4 += 0.15 * G;
	}
	if(-50 > local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] >= -230)
	{
		joint4 -= 0.05 * G;
	}
	if(-231 >= local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] >= -422)
	{
		joint4 -= 0.1 * G;
	}	
	if(-423 >= local_rc_ctrl->rc.ch[0] &&local_rc_ctrl->rc.ch[0] >= -660)
	{
		joint4 -= 0.15 * G;
	}


	
	motor_joint_control_2(joint4, joint5);//分控制2	

}




void elevator_3508(void)//底盘移动同时控制电梯
{		
//	joint6 = local_rc_ctrl->rc.ch[1] / 20;	//右上摇杆控制  电梯


	if(50 < local_rc_ctrl->rc.ch[1] &&local_rc_ctrl->rc.ch[1] <= 230)
	{
		joint6 += 0.05 * E;
	}
	if(231 <= local_rc_ctrl->rc.ch[1] &&local_rc_ctrl->rc.ch[1] <= 422)
	{
		joint6 += 0.1 * E;
	}	
	if(423 <= local_rc_ctrl->rc.ch[1] &&local_rc_ctrl->rc.ch[1] <= 660)
	{
		joint6 += 0.15 * E;
	}
	if(-50 > local_rc_ctrl->rc.ch[1] &&local_rc_ctrl->rc.ch[1] >= -230)
	{
		joint6 -= 0.05 * E;
	}
	if(-231 >= local_rc_ctrl->rc.ch[1] &&local_rc_ctrl->rc.ch[1] >= -422)
	{
		joint6 -= 0.1 * E;
	}	
	if(-423 >= local_rc_ctrl->rc.ch[1] &&local_rc_ctrl->rc.ch[1] >= -660)
	{
		joint6 -= 0.15 * E;
	}
	elevator(joint6);
}

void key_control_1(void)//键盘控制
{
	

	
	motor_joint_control_1(joint1, joint2, joint3 + add_value_5 * 0.75);
//	motor_joint_control_2(joint4, joint5);//分控制2	
	elevator(joint6);
}


void key_control_2(void)//键盘控制
{
	motor_joint_control_2(joint4, joint5);//分控制2	
	elevator(joint6);

}

void air_pump(void)//气泵
{
		if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_F)
		{
		osDelay(12);
		if(local_rc_ctrl->key.v & KEY_PRESSED_OFFSET_F)
		{
		pump_count ++;
			}
		}	
		
		if(judge_sensor_info.RemoteControlCommand.keyboard_value & KEY_PRESSED_OFFSET_F)
		{
		osDelay(12);
		if(judge_sensor_info.RemoteControlCommand.keyboard_value & KEY_PRESSED_OFFSET_F)
		{
		pump_count ++;
			}
		}			
		
		
			if(pump_count % 2 == 1)
			{
			HAL_GPIO_WritePin(GPIOC,GPIO_PIN_6, 1);	
			HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8, 1);	
				RGB_SetOne_Color(1,GREEN);//给一个LED装载24个颜色数据码（0码和1码）
				RGB_SetOne_Color(2,GREEN);
				RGB_SetOne_Color(3,GREEN);
				RGB_SetOne_Color(4,GREEN);
				RGB_Flush();  //刷新RGB显示
			}
			else
			{
			HAL_GPIO_WritePin(GPIOC,GPIO_PIN_6, 0);	
			HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8, 0);	
				RGB_SetOne_Color(1,NOT);//给一个LED装载24个颜色数据码（0码和1码）
				RGB_SetOne_Color(2,NOT);
				RGB_SetOne_Color(3,NOT);
				RGB_SetOne_Color(4,NOT);
				RGB_Flush();  //刷新RGB显示
			}

}



void Self_control_(void)//自定义控制器控制
{
//	motor_joint_control_1(joint1 + add_value_3, joint2 + add_value_4, joint3 + add_value_5);
//	motor_joint_control_2(joint4 + add_value_1, joint5 + add_value_2);//分控制2	
//	elevator(joint6 + add_value_6);
	

	
	motor_joint_control_1(joint1, joint2, joint3 + add_value_5 * 1.44);
	motor_joint_control_2(joint4, joint5);//分控制2	
	elevator(joint6);	
	
}

void steer_control(void)
{
	
	
	Limit_uint32(&camera_1, 125, 25);//限幅函数
	Limit_uint32(&camera_2, 125, 25);//限幅函数	

    		

			  

	HAL_TIM_PWM_Start_DMA(&htim8, TIM_CHANNEL_2, &camera_2, 1);				
//	HAL_TIM_PWM_Start_DMA(&htim8, TIM_CHANNEL_3, &camera_1, 1);	
	HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
	
	TIM8->CCR3 = camera_1;
}

